#include "Can_recive.h"

extern CAN_HandleTypeDef  hcan1,hcan2;
static CAN_TxHeaderTypeDef  Chassis_Header,Gimbal_Header;
static Revice_Motro Revice_ChassisData[4];
static Revice_Motro Revice_GimbalData[2];

static uint8_t    chassis_can_send_data[8];
static uint8_t    gimbal_can_send_data[8];
//CAN启动
void canfilter_init_start(void)
{
	CAN_FilterTypeDef can_filter_st;	                //定义过滤器结构体
    can_filter_st.FilterActivation = ENABLE;			//ENABLE使能过滤器
    can_filter_st.FilterMode = CAN_FILTERMODE_IDMASK;	//设置过滤器模式--标识符屏蔽位模式
    can_filter_st.FilterScale = CAN_FILTERSCALE_32BIT;	//过滤器的位宽 32 位
    can_filter_st.FilterIdHigh = 0x0000;				//ID高位
    can_filter_st.FilterIdLow = 0x0000;					//ID低位
    can_filter_st.FilterMaskIdHigh = 0x0000;			//过滤器掩码高位
    can_filter_st.FilterMaskIdLow = 0x0000;				//过滤器掩码低位
    
    can_filter_st.FilterBank = 0;						//过滤器组-双CAN可指定0~27
    can_filter_st.FilterFIFOAssignment = CAN_FILTER_FIFO0;	//与过滤器组管理的 FIFO
    HAL_CAN_ConfigFilter(&hcan1, &can_filter_st);		//HAL库配置过滤器函数
	
    HAL_CAN_Start(&hcan1);								//使能CAN1控制器
    HAL_CAN_ActivateNotification(&hcan1, CAN_IT_RX_FIFO0_MSG_PENDING);//使能CAN的各种中断
	
	
    can_filter_st.SlaveStartFilterBank = 14;   //双CAN模式下规定CAN的主从模式的过滤器分配，从过滤器为14
    can_filter_st.FilterBank = 14;						//过滤器组-双CAN可指定0~27
    HAL_CAN_ConfigFilter(&hcan2, &can_filter_st);		//HAL库配置过滤器函数
    HAL_CAN_Start(&hcan2);								//使能CAN2控制器
    HAL_CAN_ActivateNotification(&hcan2, CAN_IT_RX_FIFO0_MSG_PENDING);//使能CAN的各种中断
}
//接收底盘电调数据并解码
void Cha_Get_Motor_Data(uint8_t i,uint8_t data[],Revice_Motro* Revice_Data)
{
	Revice_Data[i].last_ecd = Revice_ChassisData[i].ecd;
	Revice_Data[i].ecd = (data[0]<<8)|data[1];
	Revice_Data[i].given_current = (data[4]<<8)|data[5];
	Revice_Data[i].rpm = (data[2]<<8)|data[3];
}
//接收云台电调数据并解码
void Gim_Get_Motor_Data(uint8_t i,uint8_t data[],Revice_Motro* Revice_Data)
{
	Revice_Data[i].last_ecd = Revice_GimbalData[i].ecd;
	Revice_Data[i].ecd = (data[0]<<8)|data[1];
	Revice_Data[i].given_current = (data[4]<<8)|data[5];
	Revice_Data[i].rpm = (data[2]<<8)|data[3];
}
//  CAN FIFO0的中断回调函数，在里面完成6个电机的数据的接收
void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef*hcan)
{
	CAN_RxHeaderTypeDef rx_header;
	uint8_t rx_data[6];
	if(hcan==&hcan1)
	{
		HAL_CAN_GetRxMessage(&hcan1,CAN_RX_FIFO0,&rx_header,rx_data);
		switch(rx_header.StdId)
		{
			case CHASSIS_M1_ID:
				Cha_Get_Motor_Data(CHASSIS_MOTOR1,rx_data,Revice_ChassisData);
				break;
			case CHASSIS_M2_ID:
				Cha_Get_Motor_Data(CHASSIS_MOTOR2,rx_data,Revice_ChassisData);
				break;
			case CHASSIS_M3_ID:
				Cha_Get_Motor_Data(CHASSIS_MOTOR3,rx_data,Revice_ChassisData);
				break;
			case CHASSIS_M4_ID:
				Cha_Get_Motor_Data(CHASSIS_MOTOR4,rx_data,Revice_ChassisData);
				break;
			case CAN_CATCH_MOTOR_ID:
				Gim_Get_Motor_Data(GIMBAL_MOTOR1,rx_data,Revice_GimbalData);
				break;
			case CAN_RISE_FALL_MOTOR_ID:
				Gim_Get_Motor_Data(GIMBAL_MOTOR2,rx_data,Revice_GimbalData);
				break;
		}
	}
	if(hcan==&hcan2)
	{
		HAL_CAN_GetRxMessage(&hcan2,CAN_RX_FIFO0,&rx_header,rx_data);
		switch(rx_header.StdId)
		{
			case CAN_CATCH_MOTOR_ID:
				Gim_Get_Motor_Data(GIMBAL_MOTOR1,rx_data,Revice_GimbalData);
				break;
			case CAN_RISE_FALL_MOTOR_ID:
				Gim_Get_Motor_Data(GIMBAL_MOTOR2,rx_data,Revice_GimbalData);
				break;
		}
	}
}

/*	
	2006控制底盘电机转动
*/

void CAN_cmd_chassis(int16_t M1, int16_t M2, int16_t M3, int16_t M4)
{
	uint32_t send_mail_box;
	
	Chassis_Header.StdId = ALL_CHASSIS_ID;
	Chassis_Header.DLC   = 0x08;
	Chassis_Header.RTR	 = CAN_RTR_DATA;
	Chassis_Header.IDE 	 = CAN_ID_STD;
	
	chassis_can_send_data[0]=M1>>8;
	chassis_can_send_data[1]=M1;
	
	chassis_can_send_data[2]=M2>>8;
	chassis_can_send_data[3]=M2;
	
	chassis_can_send_data[4]=M3>>8;
	chassis_can_send_data[5]=M3;
	
	chassis_can_send_data[6]=M4>>8;
	chassis_can_send_data[7]=M4;
	
	HAL_CAN_AddTxMessage(&hcan1,&Chassis_Header,chassis_can_send_data,&send_mail_box);
}

/*
	云台电机控制
*/
void CAN_cmd_gimbal(int16_t Rise_Fall, int16_t Catch)
{
    uint32_t send_mail_box;
    Gimbal_Header.StdId = CAN_GIMBAL_ALL_ID;
    Gimbal_Header.IDE = CAN_ID_STD;
    Gimbal_Header.RTR = CAN_RTR_DATA;
    Gimbal_Header.DLC = 0x08;
    gimbal_can_send_data[0] = (Rise_Fall >> 8);
    gimbal_can_send_data[1] = Rise_Fall;
    gimbal_can_send_data[2] = (Catch >> 8);
    gimbal_can_send_data[3] = Catch;
    HAL_CAN_AddTxMessage(&hcan2, &Gimbal_Header, gimbal_can_send_data, &send_mail_box);
}

/**
  * @brief          返回底盘电机 2006电机数据指针
  * @param[in]      i: 电机编号,范围[0,3]
  * @retval         电机数据指针
  */
const Revice_Motro *get_Chassis_Motor_Measure_Point(uint8_t i)
{
	return &Revice_ChassisData[(i&0x03)];
}


